﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;

using System.Timers;
using System.Xml;
using System.Threading;

namespace VehicleTest
{
    public enum ControlMode
    {
        PID,
        Trajectory,
        VirtualBar
    }

    public enum PIDControlMode
    {
        IncrementalPID,
        PControl,
        FixedSpeed
    }

    public struct TrajectoryPoint
    {
        public double X;
        public double Y;

        public TrajectoryPoint(double x, double y)
        {
            X = x;
            Y = y;
        }
    }

    public partial class VehicleControl : Form
    {
        private VehicleDetection control_detection;
        public ControlMode control_mode;
        public System.Timers.Timer control_period;
        public System.Timers.Timer updata_aim;
        private VehicleCommand control_cmd;

        private XmlDocument xml_ControlParameters = new XmlDocument();

        //全局参数
        private double coeff_SafeDistance;
        private int coeff_ControlPeriod;
        private double speed;
        private double del;

        //PID控制参数
        private double coeff_Speed_P;
        private double coeff_Speed_I;
        private double coeff_Speed_D;
        private double coeff_Servo_P;
        private double coeff_Servo_I;
        private double coeff_Servo_D;

        //速度PID控制变量
        private double[] error_speed = new double[3];
        private double delta_speed;
        private PIDControlMode pid_speedmode;
        private PIDControlMode pid_servomode;
        private double PID_FIXSPEED = 0;
        //轨迹控制参数
        private double coeff_trajectory_angle;
        private double coeff_trajectory_speed;
        private double trajectoryMargin;
        public double coeff_trajectory_PointThreshold; //记录前车轨迹的两点之间的阈值
        private double Trajectory_FIXSPEED = 0;
        private PIDControlMode trajectory_speedmode;
        private PIDControlMode trajectory_servomode;
        //轨迹控制变量
        public Queue<TrajectoryPoint> leaderPointStore = new Queue<TrajectoryPoint>();
        private double trajectory_aimX, trajectory_aimY;
        private double trajectory_curX, trajectory_curY, trajectory_curAngle;
        public double trajectory_length;

        //传感器得到的全局变量
        private double control_distance;
        private double control_angle;
        private double leader_x;
        private double leader_y;
        private double follower_x;
        private double follower_y;

        public VehicleControl(VehicleDetection vd)
        {
            InitializeComponent();

            control_detection = vd;
            control_mode = ControlMode.PID;
            control_cmd = new VehicleCommand(control_detection.mainwindows_detection);

            ControlParametersLoad("ControlParameters.xml");

            groupBox_ControlMode.Enabled = true;

            control_period = new System.Timers.Timer();
            control_period.Interval = coeff_ControlPeriod;
            control_period.Enabled = true;
            control_period.Stop();
            control_period.Elapsed += new ElapsedEventHandler(ControlPeriod_tick);

            speed = 0;
            del = 0;

            //PID控制参数初始化
            error_speed[0] = 0;
            error_speed[1] = 0;
            error_speed[2] = 0;
            pid_speedmode = PIDControlMode.IncrementalPID;
            pid_servomode = PIDControlMode.PControl;
            textBox_PIDFixedSpeed.Enabled = false;
            radioButton_PIDServoChoosePID.Enabled = false;

            //轨迹控制参数初始化
            trajectory_servomode = PIDControlMode.PControl;
            trajectory_speedmode = PIDControlMode.PControl;
            textBox_TrajectoryFixedSpeed.Enabled = false;
            radioButton_TrajectoryServoChoosePID.Enabled = false;
            radioButton_TrajectorySpeedChoosePID.Enabled = false;
            updata_aim = new System.Timers.Timer();
            updata_aim.Interval = 20;
            updata_aim.Enabled = true;
            updata_aim.Stop();
            updata_aim.Elapsed += new ElapsedEventHandler(UpdataAim_tick);     

            leaderPointStore.Enqueue(new TrajectoryPoint(coeff_SafeDistance + 100, 0));
        }

        #region 自定义函数
        private void ControlParametersLoad(string doc_name)
        {
            XmlDocument xml_ReadCalibration = new XmlDocument();
            xml_ReadCalibration.Load(control_detection.mainwindows_detection.path + "/doc/" + doc_name);
            XmlNode currentNode;

            //标定参数加载
            currentNode = xml_ReadCalibration.SelectSingleNode("//SafeDistance");
            coeff_SafeDistance = Convert.ToDouble(currentNode.InnerText);
            textBox_SafeDistance.Text = currentNode.InnerText;

            currentNode = xml_ReadCalibration.SelectSingleNode("//ControlPeriod");
            coeff_ControlPeriod = Convert.ToInt32(currentNode.InnerText);
            textBox_ControlPeriod.Text = currentNode.InnerText;

            currentNode = xml_ReadCalibration.SelectSingleNode("//PIDControlParameters/Speed/P");
            coeff_Speed_P = Convert.ToDouble(currentNode.InnerText);
            textBox_SpeedP.Text = currentNode.InnerText;

            currentNode = xml_ReadCalibration.SelectSingleNode("//PIDControlParameters/Speed/I");
            coeff_Speed_I = Convert.ToDouble(currentNode.InnerText);
            textBox_SpeedI.Text = currentNode.InnerText;

            currentNode = xml_ReadCalibration.SelectSingleNode("//PIDControlParameters/Speed/D");
            coeff_Speed_D = Convert.ToDouble(currentNode.InnerText);
            textBox_SpeedD.Text = currentNode.InnerText;

            currentNode = xml_ReadCalibration.SelectSingleNode("//PIDControlParameters/Servo/P");
            coeff_Servo_P = Convert.ToDouble(currentNode.InnerText);
            textBox_ServoP.Text = currentNode.InnerText;

            currentNode = xml_ReadCalibration.SelectSingleNode("//PIDControlParameters/Servo/I");
            coeff_Servo_I = Convert.ToDouble(currentNode.InnerText);
            textBox_ServoI.Text = currentNode.InnerText;

            currentNode = xml_ReadCalibration.SelectSingleNode("//PIDControlParameters/Servo/D");
            coeff_Servo_D = Convert.ToDouble(currentNode.InnerText);
            textBox_ServoD.Text = currentNode.InnerText;

            currentNode = xml_ReadCalibration.SelectSingleNode("//TrajectoryControl/Margin");
            trajectoryMargin = Convert.ToDouble(currentNode.InnerText);
            textBox_TrajectoryMargin.Text = currentNode.InnerText;

            currentNode = xml_ReadCalibration.SelectSingleNode("//TrajectoryControl/PointThreshold");
            coeff_trajectory_PointThreshold = Convert.ToDouble(currentNode.InnerText);
            textBox_TrajectoryPointThreshold.Text = currentNode.InnerText;

            currentNode = xml_ReadCalibration.SelectSingleNode("//TrajectoryControl/AngleP");
            coeff_trajectory_angle = Convert.ToDouble(currentNode.InnerText);
            textBox_TrajectoryAngleP.Text = currentNode.InnerText;

            currentNode = xml_ReadCalibration.SelectSingleNode("//TrajectoryControl/SpeedP");
            coeff_trajectory_speed = Convert.ToDouble(currentNode.InnerText);
            textBox_TrajectorySpeedP.Text = currentNode.InnerText;
        }

        private double cal_theta2(double x0, double y0, double x1, double y1, double angle)  //x0,y0,当前位置，x1,y1目标位置，angle陀螺仪角度。  返回角度偏差大于0左拐，小于0右拐
        {
            double angle_cal = Math.Atan2(y1 - y0, x1 - x0) * 180 / Math.PI;
            double angle_dif = angle_cal - angle;   //计算角-车体角
            if (Math.Abs(angle_dif) > 180)
            {
                if (angle_dif > 0)  //that means >180
                {
                    while (angle_dif > 180)
                    {
                        angle_dif = angle_dif - 360;
                    }
                }
                if (angle_dif < 0)  //that means >180
                {
                    while (angle_dif < -180)
                    {
                        angle_dif = angle_dif + 360;
                    }
                }
            }
            return angle_dif;
        }

        private bool IsPointArrived(double x_now, double y_now, double x_aim, double y_aim)   //判断是否走到目标点
        {
            double dis;
            dis = (y_aim - y_now) * (y_aim - y_now) + (x_aim - x_now) * (x_aim - x_now);
            if (dis < trajectoryMargin*trajectoryMargin)
                return true;
            else
                return false;
        }

        public double PointLength(TrajectoryPoint p1, TrajectoryPoint p2)
        {
            return Math.Sqrt((p1.X - p2.X) * (p1.X - p2.X) + (p1.Y - p2.Y) * (p1.Y - p2.Y));
        }

        internal void ChangeChartAxisRange(double x, double y)
        {
            if (x >= (0.8 * chart_TrajectoryState.ChartAreas[0].AxisX.Maximum))
            {
                chart_TrajectoryState.ChartAreas[0].AxisX.Maximum = x * 1.25;
            }
            if (x <= chart_TrajectoryState.ChartAreas[0].AxisX.Minimum)
            {
                chart_TrajectoryState.ChartAreas[0].AxisX.Minimum = x;
            }
            if (y >= (0.8 * chart_TrajectoryState.ChartAreas[0].AxisY.Maximum))
            {
                chart_TrajectoryState.ChartAreas[0].AxisY.Maximum = y * 1.25;
            }
            if (y <= (0.8 * chart_TrajectoryState.ChartAreas[0].AxisY.Minimum))
            {
                chart_TrajectoryState.ChartAreas[0].AxisY.Minimum = y * 1.25;
            }
        }

        private void PreClosing()
        {
            control_detection.control_open = false;
            control_detection.toolStripButton_OpenControl.Text = "打开控制";
        }

        #endregion

        protected override void OnFormClosing(FormClosingEventArgs e)
        {
            PreClosing();

            base.OnFormClosing(e);   //在MSDN中规定如果重载了OnFormClosing函数，最后要调用基类的OnFormClosing函数
        }

        #region 控件事件
        private void toolStripButton_StartControl_Click(object sender, EventArgs e)
        {
            if (toolStripButton_StartControl.Text == "开始控制")//开始控制
            {
                toolStripButton_StartControl.Text = "停止控制";
                toolStripButton_StartControl.Image = Image.FromFile(control_detection.mainwindows_detection.path + "/image/stop.png");

                groupBox_ControlMode.Enabled = false; //控制之后不能更换控制模式,除非停止控制
                control_period.Start();
            }
            else  //停止控制
            {
                toolStripButton_StartControl.Text = "开始控制";
                toolStripButton_StartControl.Image = Image.FromFile(control_detection.mainwindows_detection.path + "/image/start.png");

                control_period.Stop();
                groupBox_ControlMode.Enabled = true;

                Thread.Sleep(300);
                control_cmd.VehicleControl(0x01, 0, 0, 0x00);
            }
        }

        private void radioButton_PIDcontrol_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_PIDcontrol.Checked)
            {
                control_mode = ControlMode.PID;
                error_speed[0] = 0;
                error_speed[1] = 0;
                error_speed[2] = 0;
                speed = 0;
                del = 0;
            }
        }

        private void radioButton_TrajectoryControl_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_TrajectoryControl.Checked)
                control_mode = ControlMode.Trajectory;
        }

        private void radioButton_VirtualBar_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_VirtualBar.Checked)
                control_mode = ControlMode.VirtualBar;
        }

        private void toolStripButton_SavePara_Click(object sender, EventArgs e) //保存控制参数
        {
            xml_ControlParameters.Load(control_detection.mainwindows_detection.path + "/doc/ParametersModel.xml");

            XmlElement xml_root = xml_ControlParameters.DocumentElement;
            //增加节点
            XmlElement newPIDControl = xml_ControlParameters.CreateElement("PIDControlParameters");
            XmlElement newPIDControlSpeed = xml_ControlParameters.CreateElement("Speed");
            XmlElement newPIDControlSP = xml_ControlParameters.CreateElement("P");
            XmlElement newPIDControlSI = xml_ControlParameters.CreateElement("I");
            XmlElement newPIDControlSD = xml_ControlParameters.CreateElement("D");
            XmlElement newPIDControlServo = xml_ControlParameters.CreateElement("Servo");
            XmlElement newPIDControlDelP = xml_ControlParameters.CreateElement("P");
            XmlElement newPIDControlDelI = xml_ControlParameters.CreateElement("I");
            XmlElement newPIDControlDelD = xml_ControlParameters.CreateElement("D");
            XmlElement newTrajectoryControl = xml_ControlParameters.CreateElement("TrajectoryControl");
            XmlElement newTrajectoryMargin = xml_ControlParameters.CreateElement("Margin");
            XmlElement newTrajectoryPointThreshold = xml_ControlParameters.CreateElement("PointThreshold");
            XmlElement newTrajectoryAngleP = xml_ControlParameters.CreateElement("AngleP");
            XmlElement newTrajectorySpeedP = xml_ControlParameters.CreateElement("SpeedP");
            XmlElement newSafeDistance = xml_ControlParameters.CreateElement("SafeDistance");
            XmlElement newControlPeriod = xml_ControlParameters.CreateElement("ControlPeriod");
            

            xml_root.AppendChild(newSafeDistance);
            xml_root.AppendChild(newControlPeriod);
            xml_root.AppendChild(newPIDControl);
            xml_root.AppendChild(newTrajectoryControl);
            newPIDControl.AppendChild(newPIDControlSpeed);
            newPIDControl.AppendChild(newPIDControlServo);
            newPIDControlSpeed.AppendChild(newPIDControlSP);
            newPIDControlSpeed.AppendChild(newPIDControlSI);
            newPIDControlSpeed.AppendChild(newPIDControlSD);
            newPIDControlServo.AppendChild(newPIDControlDelP);
            newPIDControlServo.AppendChild(newPIDControlDelI);
            newPIDControlServo.AppendChild(newPIDControlDelD);
            newTrajectoryControl.AppendChild(newTrajectoryMargin);
            newTrajectoryControl.AppendChild(newTrajectoryPointThreshold);
            newTrajectoryControl.AppendChild(newTrajectoryAngleP);
            newTrajectoryControl.AppendChild(newTrajectorySpeedP);

            //元素值
            XmlText SafeDistance_text = xml_ControlParameters.CreateTextNode(textBox_SafeDistance.Text);
            XmlText ControlPeriod_text = xml_ControlParameters.CreateTextNode(textBox_ControlPeriod.Text);
            XmlText SpeedP_text = xml_ControlParameters.CreateTextNode(textBox_SpeedP.Text);
            XmlText SpeedI_text = xml_ControlParameters.CreateTextNode(textBox_SpeedI.Text);
            XmlText SpeedD_text = xml_ControlParameters.CreateTextNode(textBox_SpeedD.Text);
            XmlText ServoP_text = xml_ControlParameters.CreateTextNode(textBox_ServoP.Text);
            XmlText ServoI_text = xml_ControlParameters.CreateTextNode(textBox_ServoI.Text);
            XmlText ServoD_text = xml_ControlParameters.CreateTextNode(textBox_ServoD.Text);
            XmlText TrajectoryMargin_text = xml_ControlParameters.CreateTextNode(textBox_TrajectoryMargin.Text);
            XmlText TrajectoryPointThreshold_text = xml_ControlParameters.CreateTextNode(textBox_TrajectoryPointThreshold.Text);
            XmlText TrajectoryAngleP_text = xml_ControlParameters.CreateTextNode(textBox_TrajectoryAngleP.Text);
            XmlText TrajectorySpeedP_text = xml_ControlParameters.CreateTextNode(textBox_TrajectorySpeedP.Text);

            newSafeDistance.AppendChild(SafeDistance_text);
            newControlPeriod.AppendChild(ControlPeriod_text);
            newPIDControlSP.AppendChild(SpeedP_text);
            newPIDControlSI.AppendChild(SpeedI_text);
            newPIDControlSD.AppendChild(SpeedD_text);
            newPIDControlDelP.AppendChild(ServoP_text);
            newPIDControlDelI.AppendChild(ServoI_text);
            newPIDControlDelD.AppendChild(ServoD_text);
            newTrajectoryMargin.AppendChild(TrajectoryMargin_text);
            newTrajectoryPointThreshold.AppendChild(TrajectoryPointThreshold_text);
            newTrajectoryAngleP.AppendChild(TrajectoryAngleP_text);
            newTrajectorySpeedP.AppendChild(TrajectorySpeedP_text);

            xml_ControlParameters.Save(control_detection.mainwindows_detection.path + "/doc/ControlParameters.xml");
            MessageBox.Show("成功存储参数!");
        }

        private void button_VFParaSure_Click(object sender, EventArgs e)
        {
            coeff_SafeDistance = Convert.ToDouble(textBox_SafeDistance.Text);

            byte light_state = 0xFF;
            if (checkBox_Sensor1.Checked)
                light_state &= 0xFE;
            else
                light_state |= 0x01;

            if (checkBox_Sensor2.Checked)
                light_state &= 0xFD;
            else
                light_state |= 0x02;

            if (checkBox_Sensor3.Checked)
                light_state &= 0xFB;
            else
                light_state |= 0x04;

            if (checkBox_Sensor4.Checked)
                light_state &= 0xF7;
            else
                light_state |= 0x08;

            if (checkBox_Sensor5.Checked)
                light_state &= 0xEF;
            else
                light_state |= 0x10;

            control_cmd.VehicleControl(0x03, 0, 0, light_state);

            coeff_ControlPeriod = Convert.ToInt32(textBox_ControlPeriod.Text);
            control_period.Interval = coeff_ControlPeriod;
            
        }

        private void button_PIDsure_Click(object sender, EventArgs e) //确认控制参数
        {
            coeff_Speed_P = Convert.ToDouble(textBox_SpeedP.Text);
            coeff_Speed_I = Convert.ToDouble(textBox_SpeedI.Text);
            coeff_Speed_D = Convert.ToDouble(textBox_SpeedD.Text);
            coeff_Servo_P = Convert.ToDouble(textBox_ServoP.Text);
            coeff_Servo_I = Convert.ToDouble(textBox_ServoI.Text);
            coeff_Servo_D = Convert.ToDouble(textBox_ServoD.Text);
        }

        private void radioButton_PIDchoosePID_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_PIDSpeedChoosePID.Checked)
            {
                pid_speedmode = PIDControlMode.IncrementalPID;
                textBox_PIDFixedSpeed.Enabled = false;
                error_speed[0] = 0;
                error_speed[1] = 0;
                error_speed[2] = 0;
                speed = 0;
                del = 0;
            }
        }

        private void radioButton_PIDchooseP_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_PIDSpeedChooseP.Checked)
            {
                pid_speedmode = PIDControlMode.PControl;
                textBox_PIDFixedSpeed.Enabled = false;
            }
        }

        private void radioButton_PIDchooseFixedSpeed_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_PIDSpeedChooseFixedSpeed.Checked)
            {
                pid_speedmode = PIDControlMode.FixedSpeed;
                textBox_PIDFixedSpeed.Enabled = true;
            }
        }

        private void radioButton_PIDServoChoosePID_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_PIDServoChoosePID.Checked)
                pid_servomode = PIDControlMode.IncrementalPID;
        }

        private void radioButton_PIDServoChooseP_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_PIDSpeedChooseP.Checked)
                pid_servomode = PIDControlMode.PControl;
        }

        private void button_PIDOperatorSure_Click(object sender, EventArgs e)
        {
            PID_FIXSPEED = Convert.ToInt32(textBox_PIDFixedSpeed.Text);
        }

        private void button_TrajectoryParaSure_Click(object sender, EventArgs e)
        {
            coeff_trajectory_angle = Convert.ToDouble(textBox_TrajectoryAngleP.Text);
            coeff_trajectory_speed = Convert.ToDouble(textBox_TrajectorySpeedP.Text);
            trajectoryMargin = Convert.ToDouble(textBox_TrajectoryMargin.Text);
        }

        private void radioButton_TrajectorySpeedChoosePID_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_TrajectoryServoChoosePID.Checked)
            {
                trajectory_speedmode = PIDControlMode.IncrementalPID;
                textBox_TrajectoryFixedSpeed.Enabled = false;
            }
        }

        private void radioButton_TrajectorySpeedChooseP_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_TrajectorySpeedChooseP.Checked)
            {
                trajectory_speedmode = PIDControlMode.PControl;
                textBox_TrajectoryFixedSpeed.Enabled = false;
            }
        }

        private void radioButton_TrajectoryChooseFixedSpeed_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_TrajectoryChooseFixedSpeed.Checked)
            {
                trajectory_speedmode = PIDControlMode.FixedSpeed;
                textBox_TrajectoryFixedSpeed.Enabled = true;
            }
        }

        private void radioButton_TrajectoryServoChoosePID_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_TrajectoryServoChoosePID.Checked)
                trajectory_servomode = PIDControlMode.IncrementalPID;
        }

        private void radioButton_TrajectoryServoChooseP_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_TrajectoryServoChooseP.Checked)
                trajectory_servomode = PIDControlMode.PControl;
        }

        private void button_TrajectoryOperatorSure_Click(object sender, EventArgs e)
        {
            Trajectory_FIXSPEED = Convert.ToInt32(textBox_TrajectoryFixedSpeed.Text);
        }

        private void button_ClearData_Click(object sender, EventArgs e)
        {
            trajectory_length = coeff_SafeDistance;
            chart_TrajectoryState.Series[0].Points.Clear();
            chart_TrajectoryState.Series[1].Points.Clear();
            leaderPointStore.Clear();
        }
       
        #endregion

        #region 定时线程
        private void ControlPeriod_tick(object sender, ElapsedEventArgs e)
        {
            control_distance = control_detection.distance;
            control_angle = control_detection.angle;
            leader_x = control_detection.mainwindows_detection.leader_x_coordinate;
            leader_y = control_detection.mainwindows_detection.leader_y_coordinate;
            follower_x = control_detection.mainwindows_detection.follower_x_coordinate;
            follower_y = control_detection.mainwindows_detection.follower_y_coordinate;

            switch(control_mode)
            {
                case ControlMode.PID: //PID控制
                    {
                        switch (pid_servomode)
                        {
                            case PIDControlMode.PControl: //PID控制偏转P控制
                                {
                                    del = coeff_Servo_P * control_angle;
                                    break;
                                }
                            case PIDControlMode.IncrementalPID://PID控制偏转增量式PID控制
                                {

                                    break;
                                }
                            default:
                                break;
                        }

                        switch (pid_speedmode)
                        {
                            case PIDControlMode.PControl: //PID控制速度P控制
                                {
                                    speed = coeff_Speed_P * (control_distance - coeff_SafeDistance);
                                    break;
                                }
                            case PIDControlMode.IncrementalPID://PID控制速度增量式PID控制
                                {
                                    //增量式PID控制
                                    delta_speed = error_speed[0] * (coeff_Speed_P + coeff_Speed_I + coeff_Speed_D) - error_speed[1] * (coeff_Speed_P + 2 * coeff_Speed_D) + error_speed[2] * coeff_Speed_D;
                                    speed += delta_speed;

                                    error_speed[1] = error_speed[0];
                                    error_speed[2] = error_speed[1];
                                    error_speed[0] = control_distance - coeff_SafeDistance;
                                    break;
                                }
                            case PIDControlMode.FixedSpeed://PID控制固定速度控制
                                {
                                    speed = PID_FIXSPEED;
                                    break;
                                }
                            default:
                                break;
                        }

                        if (speed < 0)
                            speed = 0;
                        else if (speed > control_cmd.SPEED_MAX)
                            speed = control_cmd.SPEED_MAX;

                        break;
                    }
                case ControlMode.Trajectory:
                    {
                        switch (trajectory_servomode)
                        {
                            case PIDControlMode.PControl: //轨迹控制偏转P控制
                                {
                                    double angle_del = cal_theta2(trajectory_curX, trajectory_curY, trajectory_aimX, trajectory_aimY, trajectory_curAngle);
                                    del = coeff_trajectory_angle * angle_del;
                                    break;
                                }
                            case PIDControlMode.IncrementalPID://轨迹控制偏转增量式PID控制
                                {

                                    break;
                                }
                            default:
                                break;
                        }

                        switch (trajectory_speedmode)
                        {
                            case PIDControlMode.PControl: //轨迹控制速度P控制
                                {
                                    speed = coeff_trajectory_speed * (trajectory_length - coeff_SafeDistance);
                                    break;
                                } 
                            case PIDControlMode.IncrementalPID://轨迹控制速度增量式PID控制
                                {
                                    //增量式PID控制
                                    
                                    break;
                                }
                            case PIDControlMode.FixedSpeed://轨迹控制固定速度控制
                                {
                                    speed = PID_FIXSPEED;
                                    break;
                                }
                            default:
                                break;
                        }

                        if (speed < 0)
                            speed = 0;
                        else if (speed > control_cmd.SPEED_MAX)
                            speed = control_cmd.SPEED_MAX;
                        break;
                    }
                case ControlMode.VirtualBar:
                    {

                        break;
                    }
                default:
                    break;
            }

            toolStripStatusLabel_SpeedState.Text = ((int)speed).ToString();
            toolStripStatusLabel_ServoState.Text = ((int)del).ToString();
            toolStripStatusLabel_TrajectoryLength.Text = ((int)trajectory_length).ToString();
            control_cmd.VehicleControl(0x00, (int)speed, (int)del, 0x00);
        }

        private void UpdataAim_tick(object sender, ElapsedEventArgs e)
        {
            trajectory_curX = control_detection.mainwindows_detection.follower_x_coordinate;
            trajectory_curY = control_detection.mainwindows_detection.follower_y_coordinate;
            trajectory_curAngle = control_detection.mainwindows_detection.angle;

            if (leaderPointStore.Count > 0)
            {
                trajectory_aimX = leaderPointStore.ElementAt<TrajectoryPoint>(0).X;
                trajectory_aimY = leaderPointStore.ElementAt<TrajectoryPoint>(0).Y;
                if (IsPointArrived(trajectory_curX, trajectory_curY, trajectory_aimX, trajectory_aimY))
                {
                    chart_TrajectoryState.Series[1].Points.AddXY(trajectory_aimX, trajectory_aimY);
                    trajectory_length -= PointLength(leaderPointStore.ElementAt<TrajectoryPoint>(0), leaderPointStore.ElementAt<TrajectoryPoint>(1));
                    leaderPointStore.Dequeue();
                    
                }
            }
        }
        #endregion

        #region 文本格式控制
        private void TestTextPID()
        {
            double test;
            test = Convert.ToDouble(textBox_SpeedP.Text);
            test = Convert.ToDouble(textBox_SpeedI.Text);
            test = Convert.ToDouble(textBox_SpeedD.Text);
            test = Convert.ToDouble(textBox_ServoP.Text);
            test = Convert.ToDouble(textBox_ServoI.Text);
            test = Convert.ToDouble(textBox_ServoD.Text);
        }

        private void TestTextPIDControl()
        {
            Int32 test;
            test = Convert.ToInt32(textBox_PIDFixedSpeed.Text);
        }

        private void TestTextTrajectory()
        {
            double test;

            test = Convert.ToDouble(textBox_TrajectoryMargin.Text);
            test = Convert.ToDouble(textBox_TrajectorySpeedP.Text);
            test = Convert.ToDouble(textBox_TrajectoryAngleP.Text);
        }

        private void TestTextTrajectoryControl()
        {
            Int32 test;
            test = Convert.ToInt32(textBox_TrajectoryFixedSpeed.Text);
        }

        private void TestTextVFPara()
        {
            double test1;
            int test2;
            test1 = Convert.ToDouble(textBox_SafeDistance.Text);
            test2 = Convert.ToInt32(textBox_ControlPeriod.Text);
        }

        private void textBox_SafeDistance_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextVFPara();
                button_VFParaSure.Enabled = true;
                toolStripButton_SavePara.Enabled = true;
            }
            catch (FormatException)
            {
                button_VFParaSure.Enabled = false;
                toolStripButton_SavePara.Enabled = false;
            }
        }

        private void textBox_ControlPeriod_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextVFPara();
                button_VFParaSure.Enabled = true;
                toolStripButton_SavePara.Enabled = true;
            }
            catch (FormatException)
            {
                button_VFParaSure.Enabled = false;
                toolStripButton_SavePara.Enabled = false;
            }
        }

        private void textBox_SpeedP_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextPID();
                button_PIDsure.Enabled = true;
                toolStripButton_SavePara.Enabled = true;
            }
            catch (FormatException)
            {
                button_PIDsure.Enabled = false;
                toolStripButton_SavePara.Enabled = false;
            }
        }

        private void textBox_SpeedI_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextPID();
                button_PIDsure.Enabled = true;
                toolStripButton_SavePara.Enabled = true;
            }
            catch (FormatException)
            {
                button_PIDsure.Enabled = false;
                toolStripButton_SavePara.Enabled = false;
            }
        }

        private void textBox_SpeedD_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextPID();
                button_PIDsure.Enabled = true;
                toolStripButton_SavePara.Enabled = true;
            }
            catch (FormatException)
            {
                button_PIDsure.Enabled = false;
                toolStripButton_SavePara.Enabled = false;
            }
        }

        private void textBox_ServoP_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextPID();
                button_PIDsure.Enabled = true;
                toolStripButton_SavePara.Enabled = true;
            }
            catch (FormatException)
            {
                button_PIDsure.Enabled = false;
                toolStripButton_SavePara.Enabled = false;
            }
        }

        private void textBox_ServoI_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextPID();
                button_PIDsure.Enabled = true;
                toolStripButton_SavePara.Enabled = true;
            }
            catch (FormatException)
            {
                button_PIDsure.Enabled = false;
                toolStripButton_SavePara.Enabled = false;
            }
        }

        private void textBox_ServoD_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextPID();
                button_PIDsure.Enabled = true;
                toolStripButton_SavePara.Enabled = true;
            }
            catch (FormatException)
            {
                button_PIDsure.Enabled = false;
                toolStripButton_SavePara.Enabled = false;
            }
        }

        private void textBox_PIDFixedSpeed_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextPIDControl();
                button_PIDOperatorSure.Enabled = true;
            }
            catch (FormatException)
            {
                button_PIDOperatorSure.Enabled = false;
            }
        }

        private void textBox_TrajectoryFixedSpeed_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextTrajectoryControl();
                button_TrajectoryOperatorSure.Enabled = true;
            }
            catch (FormatException)
            {
                button_TrajectoryOperatorSure.Enabled = false;
            }
        }

        private void textBox_TrajectoryMargin_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextTrajectory();
                button_TrajectoryParaSure.Enabled = true;
                toolStripButton_SavePara.Enabled = true;
            }
            catch (FormatException)
            {
                button_TrajectoryParaSure.Enabled = false;
                toolStripButton_SavePara.Enabled = false;
            }
        }

        private void textBox_TrajectoryAngleP_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextTrajectory();
                button_TrajectoryParaSure.Enabled = true;
                toolStripButton_SavePara.Enabled = true;
            }
            catch (FormatException)
            {
                button_TrajectoryParaSure.Enabled = false;
                toolStripButton_SavePara.Enabled = false;
            }
        }

        private void textBox_TrajectorySpeedP_TextChanged(object sender, EventArgs e)
        {
            try
            {
                TestTextTrajectory();
                button_TrajectoryParaSure.Enabled = true;
                toolStripButton_SavePara.Enabled = true;
            }
            catch (FormatException)
            {
                button_TrajectoryParaSure.Enabled = false;
                toolStripButton_SavePara.Enabled = false;
            }
        }
        #endregion       

    }
}
